/**
 * @file AStarPlanner.cpp
 * @brief A*算法路径规划器实现
 */

#include "planners/AStarPlanner.h"
#include <queue>
#include <unordered_set>
#include <chrono>

namespace PathPlanning
{
AStarPlanner::AStarPlanner(std::shared_ptr<Map> map) : PlannerBase(map)
{
    config_ = std::make_shared<AStarConfig>();
}

PlanningResult AStarPlanner::plan(const Point& start, const Point& goal)
{
    PlanningResult result;
    auto           startTime = std::chrono::high_resolution_clock::now();

    if (!validateStartGoal(start, goal))
    {
        result.message = "Invalid start or goal position";
        return result;
    }

    auto aStarConfig = std::static_pointer_cast<AStarConfig>(config_);

    // 初始化数据结构
    std::priority_queue<std::shared_ptr<AStarNode>, std::vector<std::shared_ptr<AStarNode>>, AStarNodeCompare> openSet;

    std::unordered_map<GridCell, std::shared_ptr<AStarNode>, GridCellHash> allNodes;
    std::unordered_set<GridCell, GridCellHash>                             closedSet;

    // 创建起点节点
    GridCell startCell = map_->worldToGrid(start);
    auto     startNode = std::make_shared<AStarNode>(startCell, start);
    startNode->gCost   = 0.0;
    startNode->fCost   = calculateHeuristic(start, goal) * aStarConfig->weight;

    openSet.push(startNode);
    allNodes[startCell] = startNode;

    GridCell goalCell = map_->worldToGrid(goal);

    int       iterations    = 0;
    const int maxIterations = 100000;

    while (!openSet.empty() && iterations < maxIterations)
    {
        iterations++;

        auto currentNode = openSet.top();
        openSet.pop();

        GridCell currentCell = currentNode->gridCell;

        if (closedSet.find(currentCell) != closedSet.end())
        {
            continue;
        }

        closedSet.insert(currentCell);
        result.exploredNodes.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));

        // 检查是否到达目标
        if (currentCell.x == goalCell.x && currentCell.y == goalCell.y)
        {
            result.success = true;
            result.path    = reconstructPath(allNodes, currentNode);
            break;
        }

        // 可视化回调
        if (vizCallback_ && iterations % 1000 == 0)
        {
            result.iterations = iterations;
            vizCallback_(result);
        }

        // 遍历邻居节点
        auto neighbors = getNeighbors(currentCell, aStarConfig->eightConnected);
        for (const auto& neighborCell : neighbors)
        {
            if (map_->isObstacle(neighborCell.x, neighborCell.y) || closedSet.find(neighborCell) != closedSet.end())
            {
                continue;
            }

            Point  neighborWorld = map_->gridToWorld(neighborCell);
            double movementCost  = currentNode->gCost + euclideanDistance(currentNode->position, neighborWorld);

            auto neighborIt = allNodes.find(neighborCell);
            if (neighborIt == allNodes.end())
            {
                // 新节点
                auto neighborNode    = std::make_shared<AStarNode>(neighborCell, neighborWorld);
                neighborNode->gCost  = movementCost;
                neighborNode->fCost  = movementCost + calculateHeuristic(neighborWorld, goal) * aStarConfig->weight;
                neighborNode->parent = currentNode;

                openSet.push(neighborNode);
                allNodes[neighborCell] = neighborNode;
            }
            else
            {
                // 已有节点，检查是否需要更新
                auto neighborNode = neighborIt->second;
                if (movementCost < neighborNode->gCost)
                {
                    neighborNode->gCost  = movementCost;
                    neighborNode->fCost  = movementCost + calculateHeuristic(neighborWorld, goal) * aStarConfig->weight;
                    neighborNode->parent = currentNode;

                    openSet.push(neighborNode);
                }
            }
        }
    }

    auto endTime        = std::chrono::high_resolution_clock::now();
    result.planningTime = std::chrono::duration<double>(endTime - startTime).count();
    result.iterations   = iterations;

    if (!result.success)
    {
        result.message = "Path not found within iteration limit";
    }
    else
    {
        // 计算路径长度
        for (size_t i = 1; i < result.path.size(); ++i)
        {
            result.pathLength += euclideanDistance(result.path[i - 1].toPoint(), result.path[i].toPoint());
        }
        result.message = "Path found successfully";
    }

    return result;
}

double AStarPlanner::calculateHeuristic(const Point& from, const Point& to)
{
    auto aStarConfig = std::static_pointer_cast<AStarConfig>(config_);

    if (aStarConfig->heuristicType == "manhattan")
    {
        return manhattanDistance(from, to);
    }
    else
    {
        return euclideanDistance(from, to);
    }
}

std::vector<Pose>
AStarPlanner::reconstructPath(const std::unordered_map<GridCell, std::shared_ptr<AStarNode>, GridCellHash>& allNodes,
                              const std::shared_ptr<AStarNode>&                                             endNode)
{
    std::vector<Pose> path;
    auto              currentNode = endNode;

    while (currentNode != nullptr)
    {
        path.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));
        if (currentNode->parent)
        {
            currentNode = std::static_pointer_cast<AStarNode>(currentNode->parent);
        }
        else
        {
            break;
        }
    }

    std::reverse(path.begin(), path.end());
    return smoothPath(path);
}

} // namespace PathPlanning